
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       gcs.h
  * @author     baiyang
  * @date       2022-11-4
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include "gcs_mavlink.h"
#include "gcs_text_queue.h"

#include "mav_routing.h"

#include <rtconfig.h>

#include "mb_message.h"

#include <common/microbee.h>
#include <parameter/param.h>
#include <common/gp_config.h>
#include <mission/mb_mission.h>
#include <rtc/jitter_correction.h>
#include <common/utility/bitmask.h>
#include <common/location/location.h>
#include <sensor_gps/sensor_gps_backend.h>
/*-----------------------------------macro------------------------------------*/
#if !defined(GCS_SEND_TEXT)
#define GCS_SEND_TEXT(severity, format, ...)     gcs_send_text(severity, format, ##__VA_ARGS__)
#endif

#define gcs_no_bucket_to_send     ((uint8_t)-1)
#define gcs_no_message_to_send    ((enum mb_message)-1)

#define GCS_DEBUG_SEND_MESSAGE_TIMINGS 0

#if !defined(HAL_HIGH_LATENCY2_ENABLED)
#if defined(HAL_MINIMIZE_FEATURES)
#define HAL_HIGH_LATENCY2_ENABLED 0
#else
#define HAL_HIGH_LATENCY2_ENABLED 1
#endif
#endif

#ifndef HAL_MAVLINK_INTERVALS_FROM_FILES_ENABLED
//#define HAL_MAVLINK_INTERVALS_FROM_FILES_ENABLED (defined(HAVE_FILESYSTEM_SUPPORT) && BOARD_FLASH_SIZE > 1024)
#define HAL_MAVLINK_INTERVALS_FROM_FILES_ENABLED 0
#endif

// important note: despite the names, these messages do NOT check to
// see if the payload will fit in the buffer.  They check to see if
// the packed message along with any channel overhead will fit.

// PAYLOAD_SIZE returns the amount of space required to send the
// mavlink message with id id on channel chan.  Mavlink2 has higher
// overheads than mavlink1, for example.

// check if a message will fit in the payload space available
#define MAV_PAYLOAD_SIZE(chan, id) ((unsigned)(gcsmav_packet_overhead_chan(chan)+MAVLINK_MSG_ID_ ## id ## _LEN))

// HAVE_PAYLOAD_SPACE evaluates to an expression that can be used
// anywhere in the code to determine if the mavlink message with ID id
// can currently fit in the output of _chan.  Note the use of the ","
// operator here to increment a counter.
#define MAV_HAVE_PAYLOAD_SPACE(_chan, id) (mavcomm_get_txspace(_chan) >= MAV_PAYLOAD_SIZE(_chan, id) ? true : (gcs_out_of_space_to_send(_chan), false))

// CHECK_PAYLOAD_SIZE - macro which may only be used within a
// GCS_MAVLink object's methods.  It inserts code which will
// immediately return false from the current function if there is no
// room to fit the mavlink message with id id on the current object's
// output
#define MAV_CHECK_PAYLOAD_SIZE(gcsmav, id) if (gcsmav_txspace(gcsmav) < (unsigned)(gcsmav_packet_overhead(gcsmav)+MAVLINK_MSG_ID_ ## id ## _LEN)) { gcs_out_of_space_to_send(gcsmav->chan); return false; }

// CHECK_PAYLOAD_SIZE2 - macro which inserts code which will
// immediately return false from the current function if there is no
// room to fit the mavlink message with id id on the mavlink output
// channel "chan".  It is expecting there to be a "chan" variable in
// scope.
#define MAV_CHECK_PAYLOAD_SIZE2(gcsmav, id) if (!MAV_HAVE_PAYLOAD_SPACE(gcsmav->chan, id)) return false

// CHECK_PAYLOAD_SIZE2_VOID - macro which inserts code which will
// immediately return from the current (void) function if there is no
// room to fit the mavlink message with id id on the mavlink output
// channel "chan".
#define MAV_CHECK_PAYLOAD_SIZE2_VOID(chan, id) if (!MAV_HAVE_PAYLOAD_SPACE(chan, id)) return

// convenience macros for defining which ap_message ids are in which streams:
#define MAV_STREAM_ENTRY(stream_name)           \
    {                                           \
        stream_name,               \
        stream_name ## _msgs,                   \
        ARRAY_SIZE(stream_name ## _msgs)        \
    }
#define MAV_STREAM_TERMINATOR { (enum mav_streams)0, NULL, 0 }

/*----------------------------------typedef-----------------------------------*/
// NOTE! The streams enum below and the
// set of AP_Int16 stream rates _must_ be
// kept in the same order
enum mav_streams {
    STREAM_RAW_SENSORS,
    STREAM_EXTENDED_STATUS,
    STREAM_RC_CHANNELS,
    STREAM_RAW_CONTROLLER,
    STREAM_POSITION,
    STREAM_EXTRA1,
    STREAM_EXTRA2,
    STREAM_EXTRA3,
    STREAM_PARAMS,
    STREAM_ADSB,
    NUM_STREAMS
};

#define GCS_MAVLINK_NUM_STREAM_RATES 10
typedef struct gcs_mavlink_parameters* gcs_mavlink_parameters_t;
struct gcs_mavlink_parameters {
    // saveable rate of each stream
    Param_int16        streamRates[GCS_MAVLINK_NUM_STREAM_RATES];
};

// last time we got a non-zero RSSI from RADIO_STATUS
struct LastRadioStatus {
    uint32_t remrssi_ms;
    uint8_t rssi;
    uint32_t received_ms; // time RADIO_STATUS received
};

struct stream_entries {
    const enum mav_streams stream_id;
    const enum mb_message *mb_message_ids;
    const uint8_t num_mb_message_ids;
};

struct deferred_message_t {
    enum mb_message id;
    uint16_t interval_ms;
    uint16_t last_sent_ms; // from AP_HAL::millis16()
};

struct deferred_message_bucket_t {
    struct mb_bitmask mb_message_ids;
    uint32_t bits[BITMASK_NUMBITS_TO_NUMWORDS(MAV_MSG_LAST)];
    uint16_t interval_ms;
    uint16_t last_sent_ms; // from AP_HAL::millis16()
};

// structure stored in FRAM
struct SigningKey {
    uint32_t magic;
    uint64_t timestamp;
    uint8_t secret_key[32];
};

typedef struct gcs_mavlink* gcs_mavlink_t;
typedef struct gcs_mavlink_ops* gcs_mavlink_ops_t;
typedef bool (*protocol_handler_fn_t)(uint8_t, rt_device_t);

struct gcs_mavlink {
    gcs_mavlink_ops_t           ops;

    // mission item index to be sent on queued msg, delayed or not
    uint16_t mission_item_reached_index;

    mavlink_channel_t           chan;

    param_t*                    _queued_parameter;      ///< next parameter to

    // saveable rate of each stream
    Param_int16*                streamRates;

#if HAL_HIGH_LATENCY2_ENABLED
    // true if this is a high latency link
    bool is_high_latency_link;
    bool high_latency_link_enabled;
#endif // HAL_HIGH_LATENCY2_ENABLED

    uint32_t                    last_heartbeat_time; // milliseconds

    struct {
        int64_t sent_ts1;
        uint32_t last_sent_ms;
        uint16_t interval_ms;   //const uint16_t interval_ms = 10000;
    } _timesync_request;

    uint32_t                    last_accel_cal_ms; // used to rate limit accel cals for bad links

    // send a (textual) message to the GCS that a received message has
    // been deprecated
    uint32_t                    last_deprecation_warning_send_time_ms;
    const char*                 last_deprecation_message;

    /// The stream we are communicating over
    rt_device_t                 _port;

    /// Perform queued sending operations
    ///
    uint16_t                    _queued_parameter_index; ///< next queued
                                                         // parameter's index
    uint16_t                    _queued_parameter_count; ///< saved count of
                                                         // parameters for
                                                         // queued send
    uint32_t                    _queued_parameter_send_time_ms;

    // number of extra ms to add to slow things down for the radio
    uint16_t                    stream_slowdown_ms;
    // last reported radio buffer percent available
    uint8_t                     last_txbuf;              // last_txbuf = 100;

    struct deferred_message_t deferred_message[3];

    // cache of which deferred message should be sent next:
    int8_t next_deferred_message_to_send_cache;

    struct deferred_message_bucket_t deferred_message_bucket[10];

    uint8_t sending_bucket_id;
    struct mb_bitmask bucket_message_ids_to_send;
    uint32_t bucket_message_ids_bits[BITMASK_NUMBITS_TO_NUMWORDS(MAV_MSG_LAST)];

    // bitmask of IDs the code has spontaneously decided it wants to
    // send out.  Examples include HEARTBEAT (gcs_send_heartbeat)
    struct mb_bitmask pushed_mb_message_ids;
    uint32_t pushed_mb_message_ids_bits[BITMASK_NUMBITS_TO_NUMWORDS(MAV_MSG_LAST)];

    // boolean that indicated that message intervals have been set
    // from streamrates:
    bool deferred_messages_initialised;

    mavlink_signing_t signing;

    // alternative protocol handler support
    struct {
        protocol_handler_fn_t handler;
        uint32_t last_mavlink_ms;
        uint32_t last_alternate_ms;
        bool active;
    } alternative;

    JitterCorrection lag_correction;
    
    // we cache the current location and send it even if the AHRS has
    // no idea where we are:
    Location global_position_current_loc;

    uint8_t  last_tx_seq;
    uint16_t send_packet_count;
    uint16_t out_of_space_to_send_count; // number of times HAVE_PAYLOAD_SPACE and friends have returned false

#if GCS_DEBUG_SEND_MESSAGE_TIMINGS
    struct {
        uint32_t longest_time_us;
        enum mb_message longest_id;
        uint32_t no_space_for_message;
        uint16_t statustext_last_sent_ms;
        uint32_t behind;
        uint32_t out_of_time;
        uint16_t fnbts_maxtime;
        uint32_t max_retry_deferred_body_us;
        uint8_t max_retry_deferred_body_type;
    } try_send_message_stats;
    uint16_t max_slowdown_ms;
#endif

    uint32_t last_mavlink_stats_logged;

    uint8_t last_battery_status_idx;

    // if we've ever sent a DISTANCE_SENSOR message out of an
    // orientation we continue to send it out, even if it is not
    // longer valid.
    uint8_t proximity_ever_valid_bitmask;

    // true if we should NOT do MAVLink on this port (usually because
    // someone's doing SERIAL_CONTROL over mavlink)
    bool _locked;
};

struct gcs_mavlink_ops {
    // packetReceived is called on any successful decode of a mavlink message
    void (*packetReceived)(gcs_mavlink_t gcsmav, const mavlink_status_t *status, const mavlink_message_t *msg);

    uint8_t (*sysid_my_gcs)(gcs_mavlink_t gcsmav);
    bool (*sysid_enforce)(gcs_mavlink_t gcsmav);

    void (*send_rangefinder)(gcs_mavlink_t gcsmav);
    void (*send_nav_controller_output)(gcs_mavlink_t gcsmav);
    void (*send_pid_tuning)(gcs_mavlink_t gcsmav);

    void (*send_attitude_target)(gcs_mavlink_t gcsmav);
    void (*send_position_target_global_int)(gcs_mavlink_t gcsmav);
    void (*send_position_target_local_ned)(gcs_mavlink_t gcsmav);

    void (*send_scaled_pressure3)(gcs_mavlink_t gcsmav); // allow sub to override this

    uint64_t (*capabilities)(gcs_mavlink_t gcsmav);

    MAV_VTOL_STATE (*vtol_state)(gcs_mavlink_t gcsmav);
    MAV_LANDED_STATE (*landed_state)(gcs_mavlink_t gcsmav);

    bool (*persist_streamrates)(gcs_mavlink_t gcsmav);

    bool (*params_ready)(gcs_mavlink_t gcsmav);

    MAV_RESULT (*handle_preflight_reboot)(gcs_mavlink_t gcsmav, const mavlink_command_long_t *packet, const mavlink_message_t *msg);
    MAV_RESULT (*handle_flight_termination)(gcs_mavlink_t gcsmav, const mavlink_command_long_t *packet);

    void (*send_banner)(gcs_mavlink_t gcsmav);
    uint32_t (*telem_delay)(gcs_mavlink_t gcsmav);

    // default empty handling of LANDING_TARGET
    void (*handle_landing_target)(gcs_mavlink_t gcsmav, const mavlink_landing_target_t *packet, uint32_t timestamp_ms);
    bool (*try_send_message)(gcs_mavlink_t gcsmav, enum mb_message id);
};

typedef struct gcs* gcs_t;
typedef struct gcs_ops* gcs_ops_t;

struct gcs {
    gcs_ops_t                     ops;

    uint32_t                      control_sensors_present;
    uint32_t                      control_sensors_enabled;
    uint32_t                      control_sensors_health;

    struct gcs_mavlink_parameters chan_parameters[MAVLINK_COMM_NUM_BUFFERS];

    uint8_t                       _num_gcs;
    gcs_mavlink_t                 _chan[MAVLINK_COMM_NUM_BUFFERS];

    char                          statustext_printf_buffer[256+1];

    // time we last saw traffic from our GCS
    uint32_t                      _sysid_mygcs_last_seen_time_ms;

    // queue of outgoing statustext messages.  Each entry consumes 58
    // bytes of RAM on stm32
    struct StatusTextQueue        _statustext_queue;

    // true if we have already allocated protocol objects:
    bool                          initialised_missionitemprotocol_objects;

    // true if update_send has ever been called:
    bool                          update_send_has_been_called;

    // handle passthru between two UARTs
    struct {
        bool enabled;
        bool timer_installed;
        rt_device_t port1;
        rt_device_t port2;
        uint32_t start_ms;
        uint32_t last_ms;
        uint32_t last_port1_data_ms;
        uint32_t baud1;
        uint32_t baud2;
        uint8_t timeout_s;
        struct rt_mutex sem;
    } _passthru;

    // this contains the index of the GCS_MAVLINK backend we will
    // first call update_send on.  It is incremented each time
    // GCS::update_send is called so we don't starve later links of
    // time in which they are permitted to send messages.
    uint8_t first_backend_to_send;
};

struct gcs_ops {
    void (*send_textv)(gcs_t gcs_singleton, MAV_SEVERITY severity, const char *fmt, va_list arg_list, uint8_t mask);

    uint16_t (*min_loop_time_remaining_for_message_send_us)(gcs_t gcs_singleton);

    bool (*vehicle_initialised)(gcs_t gcs_singleton);

    bool (*simple_input_active)(gcs_t gcs_singleton);
    bool (*supersimple_input_active)(gcs_t gcs_singleton);

    uint8_t (*sysid_this_mav)(gcs_t gcs_singleton);
    uint8_t (*sysid_my_gcs)(gcs_t gcs_singleton);

    gcs_mavlink_t (*new_gcs_mavlink_backend)(gcs_t gcs_singleton, gcs_mavlink_parameters_t params, rt_device_t uart);
    void (*update_vehicle_sensor_status_flags)(gcs_t gcs_singleton);

    enum GPS_Status (*min_status_for_gps_healthy)(gcs_t gcs_singleton);
};
/*----------------------------------variable----------------------------------*/
extern struct LastRadioStatus gcs_last_radio_status;
extern uint8_t gcs_mavlink_active;
extern uint8_t gcs_chan_is_streaming;
extern uint32_t gcs_reserve_param_space_start_ms;
extern uint8_t gcs_mavlink_private;
extern const struct stream_entries all_stream_entries[];
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
gcs_t gcs_get_singleton();

void gcs_ctor(gcs_ops_t front_ops);
void gcs_init();

gcs_mavlink_t gcs_chan(const uint8_t ofs);
const gcs_mavlink_t gcs_chan_const(const uint8_t ofs);

void gcs_create_gcs_mavlink_backend(gcs_mavlink_parameters_t params, rt_device_t uart);

void gcs_setup_console();
void gcs_setup_uarts();

uint8_t gcs_num_gcs();
void gcs_out_of_space_to_send(mavlink_channel_t chan);

/*
 * returns a mask of channels that statustexts should be sent to
 */
uint8_t gcs_statustext_send_channel_mask();

/*
  send a text message to all GCS
 */
void gcs_send_text(MAV_SEVERITY severity, const char *fmt, ...);
void gcs_send_text2(uint8_t chan, MAV_SEVERITY severity, const char *fmt, ...);
void gcs_send_textv(MAV_SEVERITY severity, const char *fmt, va_list arg_list);
void gcs_send_textv2(MAV_SEVERITY severity, const char *fmt, va_list arg_list, uint8_t dest_bitmask);
void gcs_service_statustext(void);
status_text_queue_t gcs_statustext_queue();

bool gcs_install_alternative_protocol(mavlink_channel_t c, protocol_handler_fn_t handler);

void gcs_get_sensor_status_flags(uint32_t *present, uint32_t *enabled, uint32_t *health);
void gcs_update_sensor_status_flags();
void gcs_send_to_active_channels(uint32_t msgid, const char *pkt);
void gcs_send_named_float(const char *name, float value);
#if HAL_HIGH_LATENCY2_ENABLED
void gcs_enable_high_latency_connections(bool enabled);
#endif
void gcs_send_message(enum mb_message id);
void gcs_send_message2(uint8_t chan, enum mb_message id);

void gcs_update_send();
void gcs_update_receive();

void gcs_send_mission_item_reached_message(uint16_t mission_index);

MAV_RESULT gcs_set_message_interval(uint8_t port_num, uint32_t msg_id, int32_t interval_us);
uint8_t gcs_get_channel_from_port_number(uint8_t port_num);

// last time traffic was seen from my designated GCS.  traffic
// includes heartbeats and some manual control messages.
uint32_t gcs_sysid_myggcs_last_seen_time_ms();

// called when valid traffic has been seen from our GCS
void gcs_sysid_myggcs_seen(uint32_t seen_time_ms);

// send a parameter value message to all active MAVLink connections
void gcs_send_parameter_value(const char *param_name, enum param_type_t param_type, float param_value);

static inline bool gcs_out_of_time() {
    return false;
}

// 
uint16_t gcs_min_loop_time_remaining_for_message_send_us();
bool gcs_vehicle_initialised();
bool gcs_simple_input_active();
bool gcs_supersimple_input_active();
uint8_t gcs_sysid_this_mav();
uint8_t gcs_sysid_my_gcs();
gcs_mavlink_t gcs_new_gcs_mavlink_backend(gcs_mavlink_parameters_t params, rt_device_t uart);
void gcs_update_vehicle_sensor_status_flags();
enum GPS_Status gcs_min_status_for_gps_healthy();


/// 
void gcsmav_ctor(gcs_mavlink_t gcsmav, gcs_mavlink_ops_t front_ops, gcs_mavlink_parameters_t parameters, rt_device_t uart);

bool gcsmav_init(gcs_mavlink_t gcsmav, uint8_t instance);

// accessor for uart
static inline rt_device_t gcsmav_get_uart(gcs_mavlink_t gcsmav) { return gcsmav->_port; }

// return a bitmap of active channels. Used by libraries to loop
// over active channels to send to all active channels    
uint8_t gcsmav_active_channel_mask(void);

// return a bitmap of streaming channels
uint8_t gcsmav_streaming_channel_mask(void);

// return a bitmap of private channels
uint8_t gcsmav_private_channel_mask(void);

// set a channel as private. Private channels get sent heartbeats, but
// don't get broadcast packets or forwarded packets
void gcsmav_set_channel_private(mavlink_channel_t chan);

// return true if channel is private
bool gcsmav_is_private(mavlink_channel_t chan);

// return current packet overhead for a channel
uint8_t gcsmav_packet_overhead_chan(mavlink_channel_t chan);

// this is called when we discover we'd like to send something but can't:
static inline void gcsmav_out_of_space_to_send(gcs_mavlink_t gcsmav) { gcsmav->out_of_space_to_send_count++; }

/// Check for available transmit space
uint16_t gcsmav_txspace(gcs_mavlink_t gcsmav);

static inline uint8_t gcsmav_packet_overhead(gcs_mavlink_t gcsmav) { return gcsmav_packet_overhead_chan(gcsmav->chan); }

/*
    send a statustext message to specific MAVLink connections in a bitmask

    must be called with semaphore held
 */
void gcsmav_service_statustext(gcs_mavlink_t gcsmav);

static inline bool gcsmav_is_active(gcs_mavlink_t gcsmav) {
    return gcsmav_active_channel_mask() & (1 << (gcsmav->chan-MAVLINK_COMM_0));
}

static inline bool gcsmav_is_streaming(gcs_mavlink_t gcsmav) {
    return gcsmav->sending_bucket_id != gcs_no_bucket_to_send;
}

// queue a message to be sent (try_send_message does the *actual*
// mavlink work!)
void gcsmav_send_message(gcs_mavlink_t gcsmav, enum mb_message id);
void gcsmav_send_text(gcs_mavlink_t gcsmav, MAV_SEVERITY severity, const char *fmt, ...);

static inline void gcsmav_send_message3(gcs_mavlink_t gcsmav, const char *pkt, const mavlink_msg_entry_t *entry) {
    _mav_finalize_message_chan_send(gcsmav->chan,
                                    entry->msgid,
                                    pkt,
                                    entry->min_msg_len,
                                    entry->max_msg_len,
                                    entry->crc_extra);
}

// send a mavlink_message_t out this GCS_MAVLINK connection.
// Caller is responsible for ensuring space.
static inline void gcsmav_send_message2(gcs_mavlink_t gcsmav, uint32_t msgid, const char *pkt) {
    const mavlink_msg_entry_t *entry = mavlink_get_msg_entry(msgid);
    if (entry == NULL) {
        return;
    }
    gcsmav_send_message3(gcsmav, pkt, entry);
}

#if HAL_HIGH_LATENCY2_ENABLED
    // return true if the link should be sending. Will return false if is a high latency link AND is not active
static inline bool gcsmav_should_send(gcs_mavlink_t gcsmav) { return gcsmav->is_high_latency_link ? gcsmav->high_latency_link_enabled : true; }
#endif

void gcsmav_update_send(gcs_mavlink_t gcsmav);
void gcsmav_update_receive(gcs_mavlink_t gcsmav, uint32_t max_time_us);
void gcsmav_packetReceived(gcs_mavlink_t gcsmav, const mavlink_status_t *status, const mavlink_message_t *msg);
void gcsmav_initialise_message_intervals_from_streamrates(gcs_mavlink_t gcsmav);
bool gcsmav_get_default_interval_for_mb_message(gcs_mavlink_t gcsmav, const enum mb_message id, uint16_t *interval);
void gcsmav_initialise_message_intervals_for_stream(gcs_mavlink_t gcsmav, enum mav_streams id);
uint16_t gcsmav_get_interval_for_stream(gcs_mavlink_t gcsmav, enum mav_streams id);
bool gcsmav_set_mb_message_interval(gcs_mavlink_t gcsmav, enum mb_message id, uint16_t interval_ms);
int8_t gcsmav_get_deferred_message_index(gcs_mavlink_t gcsmav, const enum mb_message id);
void gcsmav_remove_message_from_bucket(gcs_mavlink_t gcsmav, int8_t bucket, enum mb_message id);
void gcsmav_find_next_bucket_to_send(gcs_mavlink_t gcsmav, uint16_t now16_ms);
uint16_t gcsmav_get_reschedule_interval_ms(gcs_mavlink_t gcsmav, const struct deferred_message_bucket_t *deferred);
bool gcsmav_set_mavlink_message_id_interval(gcs_mavlink_t gcsmav, const uint32_t mavlink_id, const uint16_t interval_ms);
enum mb_message gcsmav_mavlink_id_to_ap_message_id(const uint32_t mavlink_id);
int8_t gcsmav_deferred_message_to_send_index(gcs_mavlink_t gcsmav, uint16_t now16_ms);
bool gcsmav_do_try_send_message(gcs_mavlink_t gcsmav, const enum mb_message id);
bool gcsmav_telemetry_delayed(gcs_mavlink_t gcsmav);
bool gcsmav_TrySendMessage(gcs_mavlink_t gcsmav, const enum mb_message id);
enum mb_message gcsmav_next_deferred_bucket_message_to_send(gcs_mavlink_t gcsmav, uint16_t now16_ms);
bool gcsmav_accept_packet(gcs_mavlink_t gcsmav, const mavlink_status_t *status, const mavlink_message_t *msg);

// reset a message interval via mavlink:
MAV_RESULT gcsmav_handle_command_set_message_interval(gcs_mavlink_t gcsmav, const mavlink_command_long_t *packet);
MAV_RESULT gcsmav_set_message_interval(gcs_mavlink_t gcsmav, uint32_t msg_id, int32_t interval_us);
MAV_RESULT gcsmav_handle_command_get_message_interval(gcs_mavlink_t gcsmav, const mavlink_command_long_t *packet);
bool gcsmav_get_mb_message_interval(gcs_mavlink_t gcsmav, enum mb_message  id, uint16_t *interval_ms);

// handle messages which don't require vehicle specific data
MAV_RESULT gcsmav_handle_command_request_message(gcs_mavlink_t gcsmav, const mavlink_command_long_t *packet);
void gcsmav_handle_message(gcs_mavlink_t gcsmav, const mavlink_message_t *msg);
void gcsmav_handle_common_message(gcs_mavlink_t gcsmav, const mavlink_message_t *msg);
void gcsmav_handle_heartbeat(gcs_mavlink_t gcsmav, const mavlink_message_t *msg);
void gcsmav_handle_timesync(gcs_mavlink_t gcsmav, const mavlink_message_t *msg);
void gcsmav_handle_send_autopilot_version(gcs_mavlink_t gcsmav, const mavlink_message_t *msg);
void gcsmav_handle_system_time_message(const mavlink_message_t *msg);
void gcsmav_handle_landing_target2(gcs_mavlink_t gcsmav, const mavlink_message_t *msg);
uint32_t gcsmav_correct_offboard_timestamp_usec_to_ms(gcs_mavlink_t gcsmav, uint64_t offboard_usec, uint16_t payload_size);
void gcsmav_handle_set_gps_global_origin(gcs_mavlink_t gcsmav, const mavlink_message_t *msg);
void gcsmav_set_ekf_origin(gcs_mavlink_t gcsmav, const Location* loc);
void gcsmav_handle_command_long(gcs_mavlink_t gcsmav, const mavlink_message_t *msg);
void gcsmav_handle_radio_status(gcs_mavlink_t gcsmav, const mavlink_message_t *msg);
MAV_RESULT gcsmav_handle_command_get_home_position(gcs_mavlink_t gcsmav, const mavlink_command_long_t *packet);

/*
  send a MAVLink message to all components with this vehicle's system id
  This is a no-op if no routes to components have been learned
*/
static inline void gcsmav_send_to_components(uint32_t msgid, const char *pkt, uint8_t pkt_len) { mavrouting_send_to_components(msgid, pkt, pkt_len); }

/*
  allow forwarding of packets / heartbeats to be blocked as required by some components to reduce traffic
*/
static inline void gcsmav_disable_channel_routing(mavlink_channel_t chan) { mavrouting_get_singleton()->no_route_mask |= (1U<<(chan-MAVLINK_COMM_0)); }

/*
  search for a component in the routing table with given mav_type and retrieve it's sysid, compid and channel
  returns if a matching component is found
 */
static inline bool gcsmav_find_by_mavtype(uint8_t mav_type, uint8_t *sysid, uint8_t *compid, mavlink_channel_t *channel) { return mavrouting_find_by_mavtype(mav_type, sysid, compid, channel); }

/*
  search for the first vehicle or component in the routing table with given mav_type and component id and retrieve its sysid and channel
  returns true if a match is found
 */
static inline bool gcsmav_find_by_mavtype_and_compid(uint8_t mav_type, uint8_t compid, uint8_t *sysid, mavlink_channel_t *channel) { return mavrouting_find_by_mavtype_and_compid(mav_type, compid, sysid, channel); }

///
// lock a channel, preventing use by MAVLink
static inline void gcsmav_lock(gcs_mavlink_t gcsmav, bool _lock) { gcsmav->_locked = _lock; }
// returns true if this channel isn't available for MAVLink
static inline bool gcsmav_locked(gcs_mavlink_t gcsmav) { return gcsmav->_locked; }

void gcsmav_send_timesync(gcs_mavlink_t gcsmav);
uint64_t gcsmav_timesync_receive_timestamp_ns(gcs_mavlink_t gcsmav);
uint64_t gcsmav_timesync_timestamp_ns(gcs_mavlink_t gcsmav);
void gcsmav_send_scaled_pressure(gcs_mavlink_t gcsmav);
void gcsmav_send_scaled_pressure2(gcs_mavlink_t gcsmav);
void gcsmav_SendScaledPressure3(gcs_mavlink_t gcsmav);
void gcsmav_send_attitude(gcs_mavlink_t gcsmav);
void gcsmav_send_attitude_quaternion(gcs_mavlink_t gcsmav);
uint64_t gcsmavCapabilities(gcs_mavlink_t gcsmav);
void gcsmav_SendBanner();
void gcsmav_send_global_position_int(gcs_mavlink_t gcsmav);
void gcsmav_send_vfr_hud(gcs_mavlink_t gcsmav);
void gcsmav_send_heartbeat(gcs_mavlink_t gcsmav);
void gcsmav_send_hwstatus(gcs_mavlink_t gcsmav);
void gcsmav_send_home_position(gcs_mavlink_t gcsmav);
void gcsmav_send_gps_global_origin(gcs_mavlink_t gcsmav);
void gcsmav_send_meminfo(gcs_mavlink_t gcsmav);
void gcsmav_send_system_time(gcs_mavlink_t gcsmav);
void gcsmav_send_mavlink_gps_raw(gcs_mavlink_t gcsmav);
void gcsmav_send_local_position(gcs_mavlink_t gcsmav);
void gcsmav_send_power_status(gcs_mavlink_t gcsmav);
void gcsmav_send_rc_channels(gcs_mavlink_t gcsmav);
bool gcsmav_sending_mavlink1(gcs_mavlink_t gcsmav);
void gcsmav_send_rc_channels_raw(gcs_mavlink_t gcsmav);
void gcsmav_send_raw_imu(gcs_mavlink_t gcsmav);
void gcsmav_send_scaled_imu(gcs_mavlink_t gcsmav, uint8_t instance, void (*send_fn)(mavlink_channel_t chan, uint32_t time_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag, int16_t temperature));
void gcsmav_send_servo_output_raw(gcs_mavlink_t gcsmav);
void gcsmav_send_sys_status(gcs_mavlink_t gcsmav);
void gcsmav_send_extended_sys_state(gcs_mavlink_t gcsmav);
void gcsmav_send_vibration(gcs_mavlink_t gcsmav);
void gcsmav_send_autopilot_version(gcs_mavlink_t gcsmav);
bool gcsmav_try_send_mission_message(gcs_mavlink_t gcsmav, const enum mb_message id);
void gcsmav_send_accelcal_vehicle_position(gcs_mavlink_t gcsmav, uint32_t position);

/// gcsmav param
MAV_PARAM_TYPE gcsmav_mav_param_type(enum param_type_t t);
void gcsmav_queued_param_send(gcs_mavlink_t gcsmav);
bool gcsmav_have_flow_control(gcs_mavlink_t gcsmav);
void gcsmav_handle_request_data_stream(gcs_mavlink_t gcsmav, const mavlink_message_t *msg);
void gcsmav_handle_param_request_list(gcs_mavlink_t gcsmav, const mavlink_message_t *msg);
void gcsmav_handle_param_request_read(gcs_mavlink_t gcsmav, const mavlink_message_t *msg);
void gcsmav_handle_param_set(gcs_mavlink_t gcsmav, const mavlink_message_t *msg);
void gcsmav_send_parameter_value(gcs_mavlink_t gcsmav, const char *param_name, enum param_type_t param_type, float param_value);
void gcsmav_handle_common_param_message(gcs_mavlink_t gcsmav, const mavlink_message_t *msg);

/// signing
void gcsmav_handle_setup_signing(gcs_mavlink_t gcsmav, const mavlink_message_t *msg);
bool gcsmav_signing_key_save(const struct SigningKey *key);
bool gcsmav_signing_key_load(struct SigningKey *key);
void gcsmav_load_signing_key(gcs_mavlink_t gcsmav);
bool gcsmav_signing_enabled(gcs_mavlink_t gcsmav);
void gcsmav_save_signing_timestamp(bool force_save_now);
// update signing timestamp on GPS lock
void gcsmav_update_signing_timestamp(uint64_t timestamp_usec);

/// gcsmav ops
static inline void gcsmav_packet_received(gcs_mavlink_t gcsmav, const mavlink_status_t *status, const mavlink_message_t *msg) {
    if (gcsmav->ops != NULL && gcsmav->ops->packetReceived != NULL) {
        gcsmav->ops->packetReceived(gcsmav, status, msg);
        return;
    }

    gcsmav_packetReceived(gcsmav, status, msg);
}

static inline uint8_t gcsmav_sysid_my_gcs(gcs_mavlink_t gcsmav) {
    RT_ASSERT(gcsmav->ops != RT_NULL);
    RT_ASSERT(gcsmav->ops->sysid_my_gcs != RT_NULL);

    return gcsmav->ops->sysid_my_gcs(gcsmav);
}

static inline bool gcsmav_sysid_enforce(gcs_mavlink_t gcsmav) {
    if (gcsmav->ops != NULL && gcsmav->ops->sysid_enforce != NULL) {
        return gcsmav->ops->sysid_enforce(gcsmav);
    }

    return false;
}

static inline void gcsmav_send_rangefinder(gcs_mavlink_t gcsmav) {
    if (gcsmav->ops != NULL && gcsmav->ops->send_rangefinder != NULL) {
        gcsmav->ops->send_rangefinder(gcsmav);
    }
}

static inline void gcsmav_send_nav_controller_output(gcs_mavlink_t gcsmav) {
    if (gcsmav->ops != NULL && gcsmav->ops->send_nav_controller_output != NULL) {
        gcsmav->ops->send_nav_controller_output(gcsmav);
    }
}

static inline void gcsmav_send_pid_tuning(gcs_mavlink_t gcsmav) {
    if (gcsmav->ops != NULL && gcsmav->ops->send_pid_tuning != NULL) {
        gcsmav->ops->send_pid_tuning(gcsmav);
    }
}

static inline void gcsmav_send_scaled_pressure3(gcs_mavlink_t gcsmav) {
    if (gcsmav->ops != NULL && gcsmav->ops->send_scaled_pressure3 != NULL) {
        gcsmav->ops->send_scaled_pressure3(gcsmav);
        return;
    }

    gcsmav_SendScaledPressure3(gcsmav);
}

static inline void gcsmav_send_attitude_target(gcs_mavlink_t gcsmav) {
    if (gcsmav->ops != NULL && gcsmav->ops->send_attitude_target != NULL) {
        gcsmav->ops->send_attitude_target(gcsmav);
    }
}

static inline void gcsmav_send_position_target_global_int(gcs_mavlink_t gcsmav) {
    if (gcsmav->ops != NULL && gcsmav->ops->send_position_target_global_int != NULL) {
        gcsmav->ops->send_position_target_global_int(gcsmav);
    }
}

static inline void gcsmav_send_position_target_local_ned(gcs_mavlink_t gcsmav) {
    if (gcsmav->ops != NULL && gcsmav->ops->send_position_target_local_ned != NULL) {
        gcsmav->ops->send_position_target_local_ned(gcsmav);
    }
}

static inline uint64_t gcsmav_capabilities(gcs_mavlink_t gcsmav) {
    if (gcsmav->ops != NULL && gcsmav->ops->capabilities != NULL) {
        return gcsmav->ops->capabilities(gcsmav);
    }

    return gcsmavCapabilities(gcsmav);
}

static inline MAV_VTOL_STATE gcsmav_vtol_state(gcs_mavlink_t gcsmav) {
    if (gcsmav->ops != NULL && gcsmav->ops->vtol_state != NULL) {
        return gcsmav->ops->vtol_state(gcsmav);
    }

    return MAV_VTOL_STATE_UNDEFINED;
}

static inline MAV_LANDED_STATE gcsmav_landed_state(gcs_mavlink_t gcsmav) {
    if (gcsmav->ops != NULL && gcsmav->ops->landed_state != NULL) {
        return gcsmav->ops->landed_state(gcsmav);
    }

    return MAV_LANDED_STATE_UNDEFINED;
}

static inline bool gcsmav_persist_streamrates(gcs_mavlink_t gcsmav) {
    if (gcsmav->ops != NULL && gcsmav->ops->persist_streamrates != NULL) {
        return gcsmav->ops->persist_streamrates(gcsmav);
    }

    return false;
}

static inline bool gcsmav_params_ready(gcs_mavlink_t gcsmav) {
    if (gcsmav->ops != NULL && gcsmav->ops->params_ready != NULL) {
        return gcsmav->ops->params_ready(gcsmav);
    }

    return true;
}

static inline void gcsmav_send_banner(gcs_mavlink_t gcsmav) {
    if (gcsmav->ops != NULL && gcsmav->ops->send_banner != NULL) {
        gcsmav->ops->send_banner(gcsmav);
        return;
    }

    gcsmav_SendBanner();
}

static inline uint32_t gcsmav_telem_delay(gcs_mavlink_t gcsmav) {
    RT_ASSERT(gcsmav->ops != RT_NULL);
    RT_ASSERT(gcsmav->ops->telem_delay != RT_NULL);

    return gcsmav->ops->telem_delay(gcsmav);
}

static inline void gcsmav_handle_landing_target(gcs_mavlink_t gcsmav, const mavlink_landing_target_t *packet, uint32_t timestamp_ms) {
    if (gcsmav->ops != NULL && gcsmav->ops->handle_landing_target != NULL) {
        gcsmav->ops->handle_landing_target(gcsmav, packet, timestamp_ms);
    }
}

static inline bool gcsmav_try_send_message(gcs_mavlink_t gcsmav, enum mb_message id) {
    if (gcsmav->ops != NULL && gcsmav->ops->try_send_message != NULL) {
        return gcsmav->ops->try_send_message(gcsmav, id);
    }

    return gcsmav_TrySendMessage(gcsmav, id);
}
/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



